#include "filter.h"
#include <stdlib.h>
#include <string.h>
#include <assert.h>

MovingAverageFilter* moving_average_filter_create(int size) {
    if (size <= 0) {
        return NULL;
    }
    
    MovingAverageFilter* filter = (MovingAverageFilter*)malloc(sizeof(MovingAverageFilter));
    if (!filter) {
        return NULL;
    }
    
    filter->buffer = (float*)malloc(size * sizeof(float));
    if (!filter->buffer) {
        free(filter);
        return NULL;
    }
    
    // 初始化缓冲区为0
    memset(filter->buffer, 0, size * sizeof(float));
    filter->size = size;
    filter->index = 0;
    filter->sum = 0.0f;
    filter->count = 0;
    
    return filter;
}

void moving_average_filter_destroy(MovingAverageFilter* filter) {
    if (filter) {
        free(filter->buffer);
        free(filter);
    }
}

float moving_average_filter_update(MovingAverageFilter* filter, float input) {
    if (!filter || filter->size == 0) {
        return 0.0f;
    }
    
    // 如果缓冲区已满，减去最早的值
    if (filter->count >= filter->size) {
        filter->sum -= filter->buffer[filter->index];
    }
    
    // 添加新值
    filter->buffer[filter->index] = input;
    filter->sum += input;
    
    // 更新索引
    filter->index = (filter->index + 1) % filter->size;
    
    // 更新计数
    if (filter->count < filter->size) {
        filter->count++;
    }
    
    return moving_average_filter_get_output(filter);
}

void moving_average_filter_reset(MovingAverageFilter* filter) {
    if (filter) {
        memset(filter->buffer, 0, filter->size * sizeof(float));
        filter->index = 0;
        filter->sum = 0.0f;
        filter->count = 0;
    }
}

float moving_average_filter_get_output(const MovingAverageFilter* filter) {
    if (!filter || filter->count == 0) {
        return 0.0f;
    }
    return filter->sum / filter->count;
}